/* LinkMatrices.java
 *
 * Handles computing matrices for the robot, using forward kinematics to find link locations
 */

import Jama.Matrix;

public class LinkMatrices {
    private Matrix t01, t12, t23, t34;
    private Matrix t0toEnd, t0to3, t0to2;
    
    private double rotation1, rotation2;
    private Line link1, link2, link3;

    public LinkMatrices(Line ln1, Line ln2, Line ln3) {
        link1 = ln1; link2 = ln2; link3 = ln3;
        setSlider(ln1.getStartX());
        setJ1(Math.PI);
        setJ2(0);
        setEnd();
        recalculate();
    }

    /*
     * Set matrix for sliding joint
     */
    private void setSlider(double d) {
	    double[][] array1 = {{Math.cos(0),-Math.sin(0),0,0},
                   {Math.sin(0)*Math.cos(0),Math.cos(0)*Math.cos(0),-Math.sin(0),d*-Math.sin(0)},
                   {Math.sin(0)*Math.sin(0),Math.cos(0)*Math.sin(0),Math.cos(0),Math.cos(0)*d},
                   {0,0,0,1}};
        t01 = new Matrix(array1);
    }

    /*
     * Set matrix for first rotating joint
     */
    private void setJ1(double theta) {
        double[][] array2 = {{Math.cos(theta),-Math.sin(theta),0,350},
                   {Math.sin(theta)*Math.cos(-Math.PI/2),Math.cos(theta)*Math.cos(-Math.PI/2),-Math.sin(-Math.PI/2),0*-Math.sin(-Math.PI/2)},
                   {Math.sin(theta)*Math.sin(-Math.PI/2),Math.cos(theta)*Math.sin(-Math.PI/2),Math.cos(-Math.PI/2),Math.cos(-Math.PI/2)*0},
                   {0,0,0,1}};
        t12 = new Matrix(array2);
        rotation1 = theta;
    }

    /*
     * Set matrix for second rotating joint
     */
    private void setJ2(double theta) {
        double[][] array3 = {{Math.cos(theta),-Math.sin(theta),0,100},
                   {Math.sin(theta)*Math.cos(0),Math.cos(theta)*Math.cos(0),-Math.sin(0),0*-Math.sin(0)},
                   {Math.sin(theta)*Math.sin(0),Math.cos(theta)*Math.sin(0),Math.cos(0),Math.cos(0)*0},
                   {0,0,0,1}};
        t23 = new Matrix(array3);
        rotation2 = theta;
    }

    /*
     * Set matrix for end effector
     */
    private void setEnd() {
        double[][] array4 = {{Math.cos(0),-Math.sin(0),0,75},
                   {Math.sin(0)*Math.cos(0),Math.cos(0)*Math.cos(0),-Math.sin(0),0*-Math.sin(0)},
                   {Math.sin(0)*Math.sin(0),Math.cos(0)*Math.sin(0),Math.cos(0),Math.cos(0)*0},
                   {0,0,0,1}};
        t34 = new Matrix(array4);
    }

    /*
     * Calculates the matrices and updates the coordinates.
     */
    private void recalculate() {
        t0to2 = t01.times(t12);
        t0to3 = t0to2.times(t23);
        t0toEnd = t0to3.times(t34);
        // print the matrices
        t01.print(5,3);
        t0to2.print(5,3);
        t0to3.print(5,3);
        t0toEnd.print(5,3);

        double[][] coords = {{0}, {0}, {0}, {1}};
        // link1
        Matrix tmp = new Matrix(coords);
        Matrix tmp2 = t01.times(tmp);
        link1.setStartX((int)tmp2.get(2, 0));
        link1.setEndX((int)tmp2.get(2, 0));

        // link2
        tmp2 = t0to3.times(tmp);
        link2.setStartX(link1.getEndX());
        link2.setStartY(link1.getEndY());
        link2.setEndX((int)tmp2.get(2, 0));
        link2.setEndY((int)tmp2.get(0, 0));

        // link3
        tmp2 = t0toEnd.times(tmp);
        link3.setStartX(link2.getEndX());
        link3.setStartY(link2.getEndY());
        link3.setEndX((int)tmp2.get(2, 0));
        link3.setEndY((int)tmp2.get(0, 0));
    }

    /*
     * Slide left (negative) or right (positive) by d
     */
    public void slide(double d) {
        setSlider(link1.getStartX() + d);
        recalculate();
    }

    /*
     * Rotate link 2 by theta (radians)
     */
    public void rot2(double theta) {
        setJ1(rotation1 + theta);
        recalculate();
    }

    /*
     * Rotate link 3 by theta (radians)
     */
    public void rot3(double theta) {
        setJ2(rotation2 + theta);
        recalculate();
    }

    public Line getLink1() {
        return link1;
    }

    public Line getLink2() {
        return link2;
    }

    public Line getLink3() {
        return link3;
    }
}
